#include "gyroscope.h"
#include "itg3200.h"
#include "physics.h"
#include "uart.h"
#include <stdint.h>
#include <string.h>

/* gyro offset */
double gyro_bias[3] = {0,0,0};

/* gyro readings */
int16_t readings[3] = {0,0,0};
double filter_gyro[4][3];
float integrate_1[3] = {0,0,0};
float old_readings[3][3] = {{0,0,0},{0,0,0},{0,0,0}};

gyro_sensor_t gyro = {0,0,0,0,0,0};

/**
 * Initialize gyroscope
 *
 * @param  none
 * @return none
 *
 * @brief  initialize gyroscope
 *
 */
void gyroscope_init(void)
{
    /* init hardware */
    itg3200_init();

    /* init gyroscope structure */
    gyroscope_calibrate();
}

/* samples collected for calibration */
#define GYRO_CALIBRATION_SAMPLES 600

/**
 * Calibrate gyroscope
 *
 * @param  none
 * @return none
 *
 * @brief  get a few samples and calculate middle value
 *
 */
void gyroscope_calibrate(void)
{
    uint16_t i;

    for (i = 0; i < GYRO_CALIBRATION_SAMPLES; i++)
    {
        /* read gyro sample */
        itg3200_read_xyz(readings);
        gyro_bias[0] += readings[0];
        gyro_bias[1] += readings[1];
        gyro_bias[2] += readings[2];
    }

    /* get averaged value */
    gyro_bias[0] /= GYRO_CALIBRATION_SAMPLES;
    gyro_bias[1] /= GYRO_CALIBRATION_SAMPLES;
    gyro_bias[2] /= GYRO_CALIBRATION_SAMPLES;

}

/**
 * Sample Gyroscope
 *
 * @param  none
 * @return none
 *
 * @brief  get a few samples and calculate middle value
 *
 */
void gyroscope_sample(void)
{
    double temp[3];

    itg3200_read_xyz(readings);

    memcpy(filter_gyro[3], filter_gyro[2], sizeof(double)*3);
    memcpy(filter_gyro[2], filter_gyro[1], sizeof(double)*3);
    memcpy(filter_gyro[1], filter_gyro[0], sizeof(double)*3);

    filter_gyro[0][0] = readings[0] - gyro_bias[0];
    filter_gyro[0][1] = readings[1] - gyro_bias[1];
    filter_gyro[0][2] = readings[2] - gyro_bias[2];

    temp[0] = (filter_gyro[0][0] + filter_gyro[1][0] + filter_gyro[2][0] + filter_gyro[3][0])/4;
    temp[1] = (filter_gyro[0][1] + filter_gyro[1][1] + filter_gyro[2][1] + filter_gyro[3][1])/4;
    temp[2] = (filter_gyro[0][2] + filter_gyro[1][2] + filter_gyro[2][2] + filter_gyro[3][2])/4;

    //ui_printf("%d %d %d\n", (int32_t)  temp[0], (int32_t) temp[1], (int32_t)  temp[2]);

    /* get rid off the damn noise */
    if (temp[0]>-7 && temp[0]<7)
    {
        temp[0]=0;
    }
    if (temp[1]>-7 && temp[1]<7)
    {
        temp[1]=0;
    }
    if (temp[2]>-7 && temp[2]<7)
    {
        temp[2]=0;
    }

    gyro.w_x = toRadians(temp[0] * GYROSCOPE_GAIN);
    gyro.w_y = toRadians(temp[1] * GYROSCOPE_GAIN);
    gyro.w_z = toRadians(temp[2] * GYROSCOPE_GAIN);

}

/**
 * Runge Kutta Integration of Gyroscope Values to flatten out jitter
 * @param  none
 * @return none
 *
 * @brief  integration(i) = integration(i-1) + 1⁄6 ( vali-3 + 2*vali-2 + 2*vali-1 + vali)
 *
 */
void gyroscope_integrate_runge_kutta(float dt)
{
    /* integrate */
    gyro.x_rungekutta += ((old_readings[2][0] + (2*old_readings[1][0]) + (2*old_readings[0][0]) + gyro.w_x) / 6)*dt;
    gyro.y_rungekutta += ((old_readings[2][1] + (2*old_readings[1][1]) + (2*old_readings[0][1]) + gyro.w_y) / 6)*dt;
    gyro.z_rungekutta += ((old_readings[2][2] + (2*old_readings[1][2]) + (2*old_readings[0][2]) + gyro.w_z) / 6)*dt;

    /* shift values */
    memcpy(&old_readings[2], &old_readings[1], 3*sizeof(float));
    memcpy(&old_readings[1], &old_readings[0], 3*sizeof(float));
    memcpy(&old_readings[0], &gyro.w_x, 3*sizeof(float));


}

